/* dsp_test.c */
#include "dsp_test.h"

//restrict keyword enabler
#define _RES restrict
//#define _RES

#define _SHIFT	1
#define _SLOT	4

#define IT_MIN	64
#define IT_MAX  64
#define IT_MUL	64

#define ASSERT_ALIGN(buf) _nassert((int)buf % 128 == 0)

void get0_itof(const SType * restrict in, float * restrict out, int n){
	ASSERT_ALIGN(in);
	ASSERT_ALIGN(out);

	#pragma MUST_ITERATE(IT_MIN, IT_MAX, IT_MUL)
	while(n--){
		*out++ = (float)(*in);
		in+=_SLOT;
	}
}

void get1_itof(const SType * restrict in, float * restrict out, int n){
	ASSERT_ALIGN(in);
	ASSERT_ALIGN(out);

	in+=_SHIFT;

	#pragma MUST_ITERATE(IT_MIN, IT_MAX, IT_MUL)
	while(n--){
		*out++ = (float)(*in);
		in+=_SLOT;
	}
}

void put0_ftoi(const float * restrict in, SType * restrict out, int n){
	ASSERT_ALIGN(in);
	ASSERT_ALIGN(out);

	#pragma MUST_ITERATE(IT_MIN, IT_MAX, IT_MUL)
	while(n--){
		*out = (SType)(*in++); 
		out+=_SLOT;
	}
}

void put1_ftoi(const float * restrict in, SType * restrict out, int n){
	ASSERT_ALIGN(in);
	ASSERT_ALIGN(out);

	out+=_SHIFT;

	#pragma MUST_ITERATE(IT_MIN, IT_MAX, IT_MUL)
	while(n--){
		*out = (SType)(*in++);
		out+=_SLOT;
	}
}

void dspBufClear(float * restrict buf, unsigned int len){
	ASSERT_ALIGN(buf);

	while (len--) *buf++ = 0;

}

//basic blocks
void dspDelayLineInit(DSPdelayLine *dl, float *delayline, unsigned int lenght){
	
	dspBufClear(delayline, lenght);

	dl->curpos 		= 0;
	dl->delayline 	= delayline;
	dl->lenght		= lenght;
	
}
 
float dspDelayLineGet(DSPdelayLine *dl){
	return dl->delayline[dl->curpos];
}

void dspDelayLinePut(DSPdelayLine *dl, float in){
	dl->delayline[dl->curpos] = in;
	if (++dl->curpos >= dl->lenght) dl->curpos = 0;
}

void 	dspTPDelayLineInit(DSPTPdelayLine *tdl, float *delayline, unsigned int *del, int n_tap){
	unsigned int maxdel=0;
	int i;

	for(i=0; i<n_tap; i++) if (del[i] > maxdel) maxdel = del[i];
	if (maxdel != del[0]) return;
	
	dspBufClear(delayline, maxdel);

	tdl->tap_num 	= n_tap;
	tdl->maxlen 	= maxdel;
	tdl->delayline 	= delayline;

	while (n_tap--){
		tdl->curpos[n_tap] = maxdel - del[n_tap];
	}
}

void dspTPDelayLineGet(DSPTPdelayLine *tdl, float *out){
	int i;

	for (i=0; i<tdl->tap_num; i++){
		out[i] = tdl->delayline[tdl->curpos[i]++];
		if (tdl->curpos[i] >= tdl->maxlen) tdl->curpos[i] = 0;
	}
}

void dspTPDelayLinePut(DSPTPdelayLine *tdl, float in){
	tdl->delayline[tdl->curpos[0]] = in;
}

void 	dspAllPassInit(DSPallPass *ap, float *delayline, float g, unsigned int n){
	dspBufClear(ap->dl.delayline, n);

	dspDelayLineInit(&ap->dl, delayline, n);

	ap->g = g;

}

float dspAllPassTick(DSPallPass *ap, float in){
	float d,z;

	d = dspDelayLineGet(&ap->dl);
	z = ap->g*d;
	dspDelayLinePut(&ap->dl, in+z);

	return -ap->g * (in + z) + d;
}

void dspDelay(DSPDelay *del, const float * _RES in, float * _RES out, int size){
	float 			gdir	= del->mix;
	float			gdel	= 1.0f - del->mix;
	float 			f 		= del->feedback;

	ASSERT_ALIGN(in);
	ASSERT_ALIGN(out);

	#pragma MUST_ITERATE(IT_MIN, IT_MAX, IT_MUL)
	while (size--){
		*out += (*in * gdir) + (gdel * dspDelayLineGet(&del->dl));
		dspDelayLinePut(&del->dl, *out++*f + *in++);
	}

}

void dspDelayInit(DSPDelay *del, float *delayline, unsigned int lenght, float mix, float feedback){

	dspDelayLineInit(&del->dl, delayline, lenght);

	del->mix 		= mix;
	del->feedback 	= feedback;

}

//reverb
void dspJCRevInit(DSPjcReverb *rev, float drywet, float *ap_delayline[3], float ap_g[3], unsigned int ap_n[3], float *ffcf_delayline, float ffcf_g[4], unsigned int ffcf_n[4]){
	
	int i;

	rev->drywet = drywet;

	//init allpass filters
	for (i=0; i<3; i++){
		dspAllPassInit(&rev->ap[i], ap_delayline[i], ap_g[i], ap_n[i]);
	}

	//init ffcf filters (tapped delay line + g[])
	dspTPDelayLineInit(&rev->tdl, ffcf_delayline, ffcf_n, 4);

	for (i=0; i<4; i++) rev->ffcf_g[i] = ffcf_g[i];

}

void dspJCReverb(DSPjcReverb *rev, const float * _RES input, float * _RES output, int size){
	float tdl_out[4];
	float ap_out;
	float dir_g = rev->drywet;
	float rev_g = 1.0f - rev->drywet;

	ASSERT_ALIGN(input);
	ASSERT_ALIGN(output);

	#pragma MUST_ITERATE(IT_MIN, IT_MAX, IT_MUL)
	while (size--){
		dspTPDelayLineGet(&rev->tdl, tdl_out);
		ap_out = dspAllPassTick(&rev->ap[0], *input);
		ap_out = dspAllPassTick(&rev->ap[1], ap_out);
		ap_out = dspAllPassTick(&rev->ap[2], ap_out);

		*output++ += rev_g * (	ap_out*rev->ffcf_g[0]+tdl_out[0] + 
								ap_out*rev->ffcf_g[1]+tdl_out[1] + 
								ap_out*rev->ffcf_g[2]+tdl_out[2] + 
								ap_out*rev->ffcf_g[3]+tdl_out[3]	) + *input++ * dir_g;

		dspTPDelayLinePut(&rev->tdl, ap_out);
	}

}
